%hw 6-10
%%%%弃用！！！！！！！！！！！！！！！！！！！！！！！！！！！

%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
motor_res = zeros(2,length(1:simu_step),3);
state_info = zeros(2,length(1:simu_step),3);
debug_info = zeros(2,length(1:simu_step),4);


%%
initial_state = [-pi/3 0 0; pi*2/3 0 0 ];
a_dj.reset_state(initial_state);
torque_info = zeros(2,length(1:simu_step),3);
joint_all_info = zeros(2,length(1:simu_step),3);


y = 0;
dy = 0;
j_pos_st= inverseKinematics(pos_st, y, a_dj.l,a_dj.l);%得到关节空间位置
j_pos_ed =inverseKinematics(pos_ed, y, a_dj.l,a_dj.l);%得到关节空间位置
pos1_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-j_pos_ed(1)+j_pos_st(1));
a1_value = fsolve(pos1_final,0);
j1_pos_s_traj_value = position_s_traj_info(a1_value,t_acc,t_ed,simu_period,j_pos_st(1));

pos2_final = @(a)(a*t_acc^2+t_acc*(t_ed-2*t_acc)*a-j_pos_ed(2)+j_pos_st(2));
a2_value = fsolve(pos2_final,0);
j2_pos_s_traj_value = position_s_traj_info(a2_value,t_acc,t_ed,simu_period,j_pos_st(2));


%暂时不分开处理两个关节的相应速度了，都用一样的
enable_m = repmat([1 0 0 0 0],2,1);
div_feedforward_con = doubleJointControler(a_dj,tor_motor,0.02,1,enable_m);
div_feedforward_con.pid_reset();
div_feedforward_con.pid_ki = repmat(400,2,1);

%先把pid架设好
con_value =[0;0];
calculated_torque = [0;0];
sample_time_count = sample_period;
for i = 1:simu_step
    joint_info = [j1_pos_s_traj_value(i,:);j2_pos_s_traj_value(i,:)];
    joint_all_info(:,i,:) = joint_info;
    if(sample_time_count >= sample_period)
        div_feedforward_con.set_target(joint_info);
        debug_info(:,i,:) = div_feedforward_con.get_id_con_value(a_dj,tor_motor,simu_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_dj,simu_period,con_value);
    state_info(:,i,:) = (a_dj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end
%%

figure;
yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% yyaxis right
% plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,1)-state_info(1,:,1)','b:o','MarkerIndices',1:1000:simu_step,'MarkerSize',2);
% legend('joint1','target','err','Location','southeast');
title("joint1 pos S traj follow  theta");


figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint2','target');
title("joint2 pos S traj follow  theta");


%%

figure;
% yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint1','target');
title("joint1 pos S traj follow  omega");
figure;
% yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint2','target','Location','southeast');
title("joint2 pos S traj follow  omega");



%%
figure;
% yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint1','target');
title("joint1 pos S traj follow  acc");
figure;
% yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
legend('joint2','target','Location','southeast');
title("joint2 pos S traj follow  acc");

% plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% yyaxis right
% plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1)-state_info(1,:,1)','b:o','MarkerIndices',1:1000:simu_step,'MarkerSize',2);
% hold on
% % plot(0:sample_period:(simu_step-1)*simu_period,debug_info(2,1:(sample_period/simu_period):end-1,2),'y:o','MarkerIndices',1:100:(simu_step)/(sample_period/simu_period),'MarkerSize',2);
% plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,1)-state_info(2,:,1)',':o','MarkerIndices',1:1000:simu_step,'MarkerSize',2);
%
% legend('joint 1','joint 2','target','joint 1 err','joint 2 err','Location',"southeast");
% title("pos S traj follow  theta");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% legend('joint 1','joint 2','target');
% title("pos S traj follow  omega");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,pos_s_traj_value(1:end-1,3),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
% % ylim([-1 1]);
% legend('joint 1','joint 2','target');
% title("pos S traj follow  acc");


